﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace BusLib
{
    public struct LatLong
    {
        public double latitude;
        public double longitude;

        public LatLong(double lat, double lon) 
        {
            latitude = lat;
            longitude = lon;
        }

        public double DistanceTo(LatLong p2)
        {
            return GeoUtils.distance(latitude, longitude, p2.latitude, p2.longitude, 'M');
        }
    };

    class GeoUtils
    {
        public const double EarthRadius = 3959; // miles
        // Both of these are ~ 1 mile in Seattle:
        public const double OneMileNorth = 0.01555555555552246;
        public const double OneMileEast = 0.0222222222221262;

        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        //:::                                                                         :::
        //:::  This routine calculates the distance between two points (given the     :::
        //:::  latitude/longitude of those points). It is being used to calculate     :::
        //:::  the distance between two ZIP Codes or Postal Codes using our           :::
        //:::  ZIPCodeWorld(TM) and PostalCodeWorld(TM) products.                     :::
        //:::                                                                         :::
        //:::  Definitions:                                                           :::
        //:::    South latitudes are negative, east longitudes are positive           :::
        //:::                                                                         :::
        //:::  Passed to function:                                                    :::
        //:::    lat1, lon1 = Latitude and Longitude of point 1 (in decimal degrees)  :::
        //:::    lat2, lon2 = Latitude and Longitude of point 2 (in decimal degrees)  :::
        //:::    unit = the unit you desire for results                               :::
        //:::           where: 'M' is statute miles                                   :::
        //:::                  'K' is kilometers (default)                            :::
        //:::                  'N' is nautical miles                                  :::
        //:::                                                                         :::
        //:::  United States ZIP Code/ Canadian Postal Code databases with latitude   :::
        //:::  & longitude are available at http://www.zipcodeworld.com               :::
        //:::                                                                         :::
        //:::  For enquiries, please contact sales@zipcodeworld.com                   :::
        //:::                                                                         :::
        //:::  Official Web site: http://www.zipcodeworld.com                         :::
        //:::                                                                         :::
        //:::  Hexa Software Development Center © All Rights Reserved 2004            :::
        //:::                                                                         :::
        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        public static double distance(double lat1, double lon1, double lat2, double lon2, char unit)
        {
            if (Math.Abs(lat1 - lat2) < double.Epsilon
                && Math.Abs(lon1 - lon2) < double.Epsilon)
            {
                return 0.0;
            }
            double theta = lon1 - lon2;
            double dist = Math.Sin(deg2rad(lat1)) * Math.Sin(deg2rad(lat2)) + Math.Cos(deg2rad(lat1)) * Math.Cos(deg2rad(lat2)) * Math.Cos(deg2rad(theta));
            dist = Math.Acos(dist);
            dist = rad2deg(dist);
            dist *= 60 * 1.1515;

            if (unit == 'K') {
                dist *= 1.609344;
            } else if (unit == 'N') {
            	dist *= 0.8684;
            }
            return (dist);
        }

        public static double distance(LatLong a, LatLong b)
        {
            return distance(a.latitude, a.longitude, b.latitude, b.longitude, 'M');
        }

        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        //::  This function converts decimal degrees to radians             :::
        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        private static double deg2rad(double deg)
        {
          return (deg * Math.PI / 180.0);
        }

        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        //::  This function converts radians to decimal degrees             :::
        //:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
        public static double rad2deg(double rad)
        {
          return (rad / Math.PI * 180.0);
        }

        public static double Bearing(double lat1, double lon1, double lat2, double lon2)
        {
            return Math.Atan2(Math.Cos(lat1) * Math.Sin(lat2) - Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(lon2-lon1), Math.Sin(lon2-lon1) * Math.Cos(lat2));
        }

        public static void DestinationPoint(double start_lat, double start_lon, double bearingDegrees, double distance, out double new_lat, out double new_lon)
        {
            double bearing = deg2rad(bearingDegrees);
            new_lat = Math.Asin(Math.Sin(start_lat) * Math.Cos(distance/EarthRadius) + Math.Cos(start_lat) * Math.Sin(distance/EarthRadius)*Math.Cos(bearing));
            new_lon = start_lon + Math.Atan2(Math.Cos(distance/EarthRadius)-Math.Sin(start_lat)*Math.Sin(new_lat), Math.Sin(bearing)*Math.Sin(distance/EarthRadius)*Math.Cos(start_lat));
        }

        public static double CrossTrackDistance(double bearing_to_target, double distance_to_target, double bearing_to_p2)
        {
            return Math.Asin(Math.Sin(distance_to_target/EarthRadius) * Math.Sin(bearing_to_target - bearing_to_p2)) * EarthRadius;
        }

        public static double[] CrossAndAlongTrackDistance(double bearing_to_target, double distance_to_target, double bearing_to_p2)
        {
            double crossTrackDistance = CrossTrackDistance(bearing_to_target, distance_to_target, bearing_to_p2);
            double alongTrackDistance = Math.Acos(Math.Cos(distance_to_target / EarthRadius) / Math.Cos(crossTrackDistance / EarthRadius)) * EarthRadius;

            double[] ret = new double[2];
            ret[0] = crossTrackDistance;
            ret[1] = alongTrackDistance;
            return ret;
        }

        public static double CrossTrackDistance(double p1_lat, double p1_lon, double p2_lat, double p2_lon, double target_lat, double target_lon)
        {
            if (p1_lat == target_lat && p1_lon == target_lon)
                return 0;

            double distance_to_target = distance(p1_lat, p2_lon, target_lat, target_lon, 'M');
            double bearing_to_target = Bearing(p1_lat, p1_lon, target_lat, target_lon);
            double bearing_to_p2 = Bearing(p1_lat, p1_lon, p2_lat, p2_lon);

            return CrossTrackDistance(bearing_to_target, distance_to_target, bearing_to_p2);
        }

        public static double AlongTrackDistance(double p1_lat, double p1_lon, double p2_lat, double p2_lon, double target_lat, double target_lon)
        {
            if (p1_lat == target_lat && p1_lon == target_lon)
                return 0;

            double distanceToTarget = distance(p1_lat, p2_lon, target_lat, target_lon, 'M');
            double bearing_to_target = Bearing(p1_lat, p1_lon, target_lat, target_lon);
            double bearing_to_p2 = Bearing(p1_lat, p1_lon, p2_lat, p2_lon);

            double crossTrackDistance = CrossTrackDistance(bearing_to_target, distanceToTarget, bearing_to_p2);

            return Math.Acos(Math.Cos(distanceToTarget/EarthRadius) / Math.Cos(crossTrackDistance/EarthRadius)) * EarthRadius;
        }

        public static double[] CrossAndAlongTrackDistance(double p1_lat, double p1_lon, double p2_lat, double p2_lon, double target_lat, double target_lon)
        {
            if (p1_lat == target_lat && p1_lon == target_lon)
            {
                double[] ret = {0.0, 0.0};
                return ret;
            }

            double distance_to_target = distance(p1_lat, p2_lon, target_lat, target_lon, 'M');
            double bearing_to_target = Bearing(p1_lat, p1_lon, target_lat, target_lon);
            double bearing_to_p2 = Bearing(p1_lat, p1_lon, p2_lat, p2_lon);

            return CrossAndAlongTrackDistance(bearing_to_target, distance_to_target, bearing_to_p2);
        }

        public static bool IsNearLine(LatLong p1, LatLong p2, LatLong target, double dist)
        {
            // Both of these are ~ 1 mile in Seattle:
            double latFudge = OneMileNorth * dist;
            double lonFudge = OneMileEast * dist;

            // Check bounds on a rectangle enclosing the whole line:
            if (target.latitude > Math.Max(p1.latitude, p2.latitude) + latFudge) return false;  // Too far North
            if (target.latitude < Math.Min(p1.latitude, p2.latitude) - latFudge) return false;  // Too far South

            if (target.longitude > Math.Max(p1.longitude, p2.longitude) + lonFudge) return false; // Too far East
            if (target.longitude < Math.Min(p1.longitude, p2.longitude) - lonFudge) return false; // Too far West

            return true;
        }

        public static double DegMinSec(int degrees, int minutes, int secs)
        {
            return (Math.Abs(degrees) + minutes/60.0 + secs / 3600.0) * Math.Sign(degrees);
        }
    }
}
